#include "turtlebot_tiny_slam.h"

#include <iostream>
#include <time.h>
#include <math.h>

#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/MapMetaData.h"

// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

TinySlam::TinySlam():
  m_map_to_odom(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
  m_laser_count(0), transform_thread_(NULL)
{
  //Initializations
  m_tfB = new tf::TransformBroadcaster();
  ROS_ASSERT(m_tfB);

  m_m_got_first_scan = false;
  m_got_map = false;

  ros::NodeHandle nodehandle("~");
	
  // parsing
  // Be consistent with gmapping to be able to exchange it exactly
  if(!nodehandle.getParam("base_frame", m_base_frame))
    m_base_frame = "base_link";
  if(!nodehandle.getParam("map_frame", m_map_frame))
    m_map_frame = "map";
  if(!nodehandle.getParam("odom_frame", m_odom_frame))
    m_odom_frame = "odom";

  double transform_publish_period;
  nodehandle.param("transform_publish_period", transform_publish_period, 0.05);

  double tmp;
  if(!nodehandle.getParam("map_update_interval", tmp))
    tmp = 5.0;
  m_map_update_interval.fromSec(tmp);
  
  // Parameters needed for TinySLAM
  if(!nodehandle.getParam("sigma_xy", m_sigma_xy))
    m_sigma_xy = 0.1;
  if(!nodehandle.getParam("sigma_theta", m_sigma_theta))
    m_sigma_theta = 0.35;
  if(!nodehandle.getParam("hole_width", m_hole_width))
    m_hole_width = 0.6;
  if(!nodehandle.getParam("span", m_span))
    m_span = 3;
  if(!nodehandle.getParam("delta", m_delta))
     m_delta = 0.05;
  ts_map_set_scale(MM_TO_METERS/m_delta);

  //communication stuff
  m_sst = m_node.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  m_sstm = m_node.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  m_ss = m_node.advertiseService("dynamic_map", &TinySlam::mapCallback, this);
  m_scan_filtersub = new message_filters::Subscriber<sensor_msgs::LaserScan>(m_node, "scan", 5);
  m_scan_filter = new tf::MessageFilter<sensor_msgs::LaserScan>(*m_m_scan_filtersub, m_tf, m_odom_frame, 5);
  m_scan_filter->registerCallback(boost::bind(&TinySlam::laserCallback, this, _1));
  transform_thread_ = new boost::thread(boost::bind(&TinySlam::publishLoop, this, transform_publish_period));
}

void TinySlam::publishLoop(double transform_publish_period){
  if(transform_publish_period == 0)
    return;

  ros::Rate r(1.0 / transform_publish_period);
  while(ros::ok()){
    publishTransform();
    r.sleep();
  }
}

TinySlam::~TinySlam()
{
  if(transform_thread_){
    transform_thread_->join();
    delete transform_thread_;
  }

  if (m_scan_filter)
    delete m_scan_filter;
  if (m_m_scan_filtersub)
    delete m_m_scan_filtersub;
}


bool TinySlam::getOdomPose(ts_m_positiont& ts_pose, const ros::Time &t)
{
	  tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
	                                           tf::Vector3(0,0,0)), t, m_base_frame);
	  tf::Stamped<tf::Transform> odom_pose;
	  try
	  {
	    m_tf.transformPose(m_odom_frame, ident, odom_pose);
	  }
	  catch(tf::TransformException e)
	  {
	    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
	    return false;
	  }

  double yaw = tf::getYaw(odom_pose.getRotation());

  ts_pose.x = odom_pose.getOrigin().x()*METERS_TO_MM + ((TS_MAP_SIZE/2)*m_delta*METERS_TO_MM); // convert to mm
  ts_pose.y = odom_pose.getOrigin().y()*METERS_TO_MM + ((TS_MAP_SIZE/2)*m_delta*METERS_TO_MM);
  ts_pose.theta = (yaw * 180/M_PI);

  ROS_DEBUG("ODOM POSE: %f, %f, %f", ts_pose.x, ts_pose.y, ts_pose.theta);

  return true;
}

bool TinySlam::initMapper(const sensor_msgs::LaserScan& scan)
{
	  // Get the laser's pose, relative to base.
	  tf::Stamped<tf::Pose> ident;
	  tf::Stamped<tf::Transform> laser_pose;
	  ident.setIdentity();
	  ident.frame_id_ = scan.header.frame_id;
	  ident.stamp_ = scan.header.stamp;
	  try
	  {
	    m_tf.transformPose(m_base_frame, ident, laser_pose);
	  }
	  catch(tf::TransformException e)
	  {
	    ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",e.what());
	    return false;
	  }
  // configure previous_odom
  if(!getOdomPose(m_prev_odom, scan.header.stamp))
     return false;
  m_position = m_prev_odom;

  // configure laser parameters
  m_lparams.offset = laser_pose.getOrigin().x();
  m_lparams.scan_size = scan.ranges.size();
  m_lparams.angle_min = scan.angle_min * 180/M_PI;
  m_lparams.angle_max = scan.angle_max * 180/M_PI;
  m_lparams.detection_margin = 0;
  m_lparams.distance_no_detection = scan.range_max * METERS_TO_MM;

  // new tinyslam instance
  ts_map_init(&m_ts_map);
  ts_state_init(&m_state, &m_ts_map, &m_lparams, &m_position, (int)(m_sigma_xy*1000), (int)(m_sigma_theta*180/M_PI), (int)(m_hole_width*1000), 0);
  
  ROS_INFO("Initialized with sigma_xy=%f, sigma_theta=%f, hole_width=%f, delta=%f",m_sigma_xy, m_sigma_theta, m_hole_width, m_delta);
  ROS_INFO("Initialization complete");
  return true; 
}

bool TinySlam::addScan(const sensor_msgs::LaserScan& scan, ts_m_positiont& odom_pose)
{
  // update odometry
  if(!getOdomPose(odom_pose, scan.header.stamp))
     return false;
  m_state.position.x += odom_pose.x - m_prev_odom.x;
  m_state.position.y += odom_pose.y - m_prev_odom.y;
  m_state.position.theta += odom_pose.theta - m_prev_odom.theta;
  m_prev_odom = odom_pose;

  ts_m_positiont prev = m_state.position;

  // update params -- mainly for PML
  m_lparams.scan_size = scan.ranges.size();
  m_lparams.angle_min = scan.angle_min * 180/M_PI;
  m_lparams.angle_max = scan.angle_max * 180/M_PI;

  ts_sensor_data_t data;
  data.position[0] = m_state.position;
  if(m_lparams.angle_max < m_lparams.angle_min){
    // flip data
    for(unsigned int i=0; i < scan.ranges.size(); i++)
      data.d[i] = (int) (scan.ranges[scan.ranges.size()-1-i]*METERS_TO_MM);
  }else{
    for(unsigned int i=0; i < scan.ranges.size(); i++)
      data.d[i] = (int) (scan.ranges[i]*METERS_TO_MM);
  } 
  ts_iterative_map_building(&data, &m_state);  
  ROS_DEBUG("Iterative step, %d, now at (%f, %f, %f)",m_laser_count, m_state.position.x, m_state.position.y, m_state.position.theta);
  ROS_DEBUG("Correction: %f, %f, %f", m_state.position.x - prev.x, m_state.position.y - prev.y, m_state.position.theta - prev.theta);
  
  odom_pose = m_state.position;
  return true;
}

void TinySlam::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  m_laser_count++;

  static ros::Time last_map_update(0,0);

  // We can't initialize TinySLAM until we've got the first scan
  if(!m_got_first_scan)
  {
    if(!initMapper(*scan))
      return;
    m_got_first_scan = true;
  }

  ts_m_positiont odom_pose;
  if(addScan(*scan, odom_pose))
  {
    ROS_DEBUG("scan processed");
    ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);

    tf::Stamped<tf::Pose> odom_to_map;
    try
    {
      m_tf.transformPose(m_odom_frame,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta*M_PI/180),
                                                                    btVector3((odom_pose.x-(TS_MAP_SIZE/2)*m_delta*METERS_TO_MM)*MM_TO_METERS, 
                                                                              (odom_pose.y-(TS_MAP_SIZE/2)*m_delta*METERS_TO_MM)*MM_TO_METERS, 0.0)).inverse(),
                                                                    scan->header.stamp, m_base_frame),odom_to_map);
    }
    catch(tf::TransformException e){
      ROS_ERROR("Transform from base_link to odom failed\n");
      odom_to_map.setIdentity();
    }

    // This looks a bit crazy, but localization is currently done *inside* tinyslam -- so odom->map never changes. Probably should fix that
    map_to_odom_mutex_.lock();
    map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
                                 tf::Point(      odom_to_map.getOrigin() ) ).inverse();
    map_to_odom_mutex_.unlock();

    if(!m_got_map || (scan->header.stamp - last_map_update) > m_map_update_interval)
    {
      updateMap();
      last_map_update = scan->header.stamp;
      ROS_DEBUG("Updated the map");
    }
  }
}

void TinySlam::updateMap()
{
  boost::mutex::scoped_lock(map_mutex_);

  if(!m_got_map) { // init map.info 
    m_map.map.info.resolution = m_delta;
    m_map.map.info.origin.position.x = 0.0;
    m_map.map.info.origin.position.y = 0.0;
    m_map.map.info.origin.position.z = 0.0;
    m_map.map.info.origin.orientation.x = 0.0;
    m_map.map.info.origin.orientation.y = 0.0;
    m_map.map.info.origin.orientation.z = 0.0;
    m_map.map.info.origin.orientation.w = 1.0;
  } 

  if(m_map.map.info.width != TS_MAP_SIZE || m_map.map.info.height != TS_MAP_SIZE){
    m_map.map.info.width = TS_MAP_SIZE;
    m_map.map.info.height = TS_MAP_SIZE;
    m_map.map.info.origin.position.x = -(TS_MAP_SIZE/2)*m_delta;
    m_map.map.info.origin.position.y = -(TS_MAP_SIZE/2)*m_delta;
    m_map.map.data.resize(m_map.map.info.width * m_map.map.info.height);  
  }

  //transform to occ grid map value range
  for(int x=0; x < TS_MAP_SIZE; x++)
  {
    for(int y=0; y < TS_MAP_SIZE; y++)
    {
      int occ= (int)(m_ts_map.map[ y * TS_MAP_SIZE + x]);
      if(occ == (TS_OBSTACLE+TS_NO_OBSTACLE)/2 )
        m_map.map.data[MAP_IDX(m_map.map.info.width, x, y)] = -1; //unexplored
      else if(occ < (TS_OBSTACLE+TS_NO_OBSTACLE)/2 )
        m_map.map.data[MAP_IDX(m_map.map.info.width, x, y)] = 100; //occupied
      else
        m_map.map.data[MAP_IDX(m_map.map.info.width, x, y)] = 0;
    }
  }
  m_got_map = true;

  //make sure to set the header information on the map
  m_map.map.header.stamp = ros::Time::now();
  m_map.map.header.frame_id = m_map_frame;

  m_sst.publish(m_map.map);
  m_sstm.publish(m_map.map.info);
}

bool TinySlam::mapCallback(nav_msgs::GetMap::Request  &req,
                          nav_msgs::GetMap::Response &res)
{
  boost::mutex::scoped_lock(map_mutex_);
  if(m_got_map && m_map.map.info.width && m_map.map.info.height)
  {
    res = m_map;
    return true;
  }
  else
    return false;
}

void TinySlam::publishTransform()
{
  map_to_odom_mutex_.lock();
  ros::Time m_tfexpiration = ros::Time::now() + ros::Duration(0.05);
  tfB_->sendTransform( tf::StampedTransform (map_to_odom_, ros::Time::now(), m_map_frame, m_odom_frame));
  map_to_odom_mutex_.unlock();
}

